#include "vec3.h"

#include "math.h"
#include "util/mmgr.h"		// Needs to come in early
#include "util/common_macros.h"		// Needs to come in early

using namespace math;
// Constructors

Vec3::Vec3(float _x,float _y,float _z): x(_x), y(_y), z(_z)
{
}

Vec3::Vec3(): x(0), y(0), z(0)
{
}

Vec3::Vec3(float* _Vec) : x(_Vec[0]),y(_Vec[1]),z(_Vec[2])
{

}




Vec3 IntersectPlane(Vec3 vNorm,float d,Vec3 p1,Vec3 p2)
{
	// returns the point where the line p1-p2 intersects the plane n&d
        Vec3 vDif	= p2 - p1;
        float	fDn		= vNorm^vDif;
        float	fT		= -(d+(vNorm^p1) )/fDn;
        return  p1 + (vDif*fT);
}

Vec3& Vec3::operator = ( const Vec3& _Vec) 
{
	x = _Vec.x;
	y = _Vec.y;
	z = _Vec.z;
	return *this;
}

void Vec3::Reset()
{
	x = y = z = 0.0f;
}



Vec3& Vec3::Modulate(const Vec3& _Val)
{
	x*=_Val.x;
	y*=_Val.y;
	z*=_Val.z;
	return *this;
}

Vec3	Vec3::LowVec(const Vec3& _A) const
{
	return Vec3(	UTIL_LOW(_A.x, x), 
					UTIL_LOW(_A.y, y), 
					UTIL_LOW(_A.z, z));
}

Vec3	Vec3::HighVec(const Vec3& _A) const
{
	return Vec3(	UTIL_HIGH(_A.x, x), 
					UTIL_HIGH(_A.y, y), 
					UTIL_HIGH(_A.z, z));
}


Vec3& Vec3::Normalize() 
{
    float	fDistance = Magnitude();

    if (fDistance==0) 
	{
		// Error
		fDistance = 1.0f;
	}

	float	fInvD = 1.0f / fDistance;

	x *= fInvD;
    y *= fInvD;
    z *= fInvD;
	return *this;
}


int Vec3::Concurrent(const Vec3& _Vec) const
{
	return(x==_Vec.x && y==_Vec.y && z==_Vec.z);
}



/*Vec3 Vec3::operator+(const Vec3& vVec1,const Vec3&  vVec2) 
{
	return Vec3( vVec1.x + vVec2.x,
					vVec1.y + vVec2.y,
					vVec1.z + vVec2.z);
}

Vec3 Vec3::operator-(const Vec3&  vVec1,const Vec3&  vVec2) 
{
	return Vec3( vVec1.x - vVec2.x,
					vVec1.y - vVec2.y,
					vVec1.z-vVec2.z);
}*/
/*
Vec3 Vec3::operator-(const Vec3&  vVec)  const
{
	return Vec3( -vVec.x,
					-vVec.y,
					-vVec.z);
}

/*Vec3 Vec3::operator*(const Vec3&  vVec1,float fScalar)   
{
	return Vec3( vVec1.x * fScalar,
					vVec1.y * fScalar,
					vVec1.z * fScalar);
}

Vec3 Vec3::operator*(float fScalar, const Vec3&  vVec1)  
{
	return Vec3(	vVec1.x * fScalar,
					vVec1.y * fScalar,
					vVec1.z * fScalar);
}*/

Vec3 Vec3::operator / (float fScalar)   const
{
	float Div = (float)(1.0f / fScalar);
	return Vec3(	x * Div,
					y * Div,
					z * Div);
}



//Dotprod
float  operator^(const Vec3&  vVec1,const Vec3&  vVec2) 
{
	return	vVec1.x * vVec2.x 
		+	vVec1.y * vVec2.y 
		+	vVec1.z * vVec2.z;
}

//Crossproduct
Vec3 operator*(const Vec3&  vVec1,const Vec3&  vVec2) 
{
    return Vec3(
                vVec1.y * vVec2.z - vVec1.z*vVec2.y,
                vVec1.z * vVec2.x - vVec1.x*vVec2.z,
                vVec1.x * vVec2.y - vVec1.y*vVec2.x);
}
Vec3 Vec3::operator * ( const Vec3& vVec2) const
{
    return Vec3(
               y * vVec2.z - z*vVec2.y,
               z * vVec2.x - x*vVec2.z,
               x * vVec2.y - y*vVec2.x);
}


// Op= functions

Vec3& Vec3::operator+= ( const Vec3& _Vec) 
{
	x += _Vec.x;
	y += _Vec.y;
	z += _Vec.z;
      return ( *this ) ;
}

Vec3& Vec3::operator-= ( const Vec3& _Vec) 
{
	x -= _Vec.x;
	y -= _Vec.y;
	z -= _Vec.z;
    return ( *this ) ;
}

Vec3& Vec3::operator*= ( const float& _f) 
{
	x *= _f;
	y *= _f;
	z *= _f;

    return ( *this ) ;
}


Vec3& Vec3::operator/= ( const float& _f) 
{
	float fInv = 1.0f / _f;
	x *= fInv;
	y *= fInv; 
	z *= fInv;

    return ( *this ) ;

}

Vec3 operator * ( float c, const Vec3& v)
{
	return Vec3(v.x*c, v.y*c, v.z*c);
}


math::Vec3 LerpVec3(const math::Vec3& From, const math::Vec3& To, float t)
{
	if (t < 0.0f) t = 0.0f;
	if (t > 1.0f) t = 1.0f;

	Vec3	Delta = To - From;
	return From + (Delta*t);
}
